﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace IronWinter
{
    abstract class Collision
    {
        /*BUGS I FORSEE
         * 
         * 1. Clicking close to a cliff or building from the top or left will prevent a unit from
         *      ever reaching its destination, (assuming the unit is bigger than 1 collision unit).
         * 
         * 2. I'm not doing trig distances between locations, I'm doing actual costs to move there.
         *      That is, I'm doing x distance + y distance.
         * 
         * 3. Rounding errors will be the death of me if a unit's top left gets too close to a
         *      mountain or building, since the unit will suddenly think it's inside the object.
         */
        public static bool[,] world;
        private const float SIFT_SPEED = 0.3f;
        private static Random rand = new Random();
        public static List<Unit> allUnits = new List<Unit>();

        /*
         * description: finds a path through the world to the target using A* search.
         * 
         * returns: true if a path was found, false otherwise.
         * 
         * side effect: changes the current path assigned to the unitToMove.
         */
        public static bool findPath(Unit unitToMove, Vector2 target)
        {
            //A bunch of stuff that I'm making up because we don't have anything to test it on.
            //The dimensions of our nav mesh.
            Point navDims = new Point(world.GetLength(0), world.GetLength(1));
            //These are the dimensions of the world in pixels.
            Point worldDims = new Point(Map.cellSize * navDims.X, Map.cellSize * navDims.Y);
            //END MADE UP STUFF
            //Change target and start world coordinates into collision coordinates.
            Node startNode = new Node(getCoords(unitToMove.Position, Map.cellSize));
            Point goal = getCoords(target, Map.cellSize);
            //Path is always valid once created since terrain cannot be placed after the game begins.
            if (unitToMove.path != null)
                return true;
            //If path is not valid, find a new one.
            List<Node> explored = new List<Node>();
            List<Node> open = new List<Node>();
            startNode.h = hCost(startNode, startNode.coordinates, goal);
            startNode.f = startNode.g + startNode.h;
            open.Add(startNode);
            //Set up my loop iteration variables
            Node currNode = null;
            float tentativeG = 0;
            int chosenIndex = 0;
            bool costImproved = false;
            //Explore nodes until a path is found or no path can be found.
            while (open.Count != 0)
            {
                currNode = open[0];
                //If true, the unit has arrived at its destination.
                if (currNode.x == goal.X && currNode.y == goal.Y)
                {
                    //Create a path.
                    List<Vector2> newPath = makePath(currNode, Map.cellSize);
                    //Prepend the actual position of the unit.
                    newPath.Insert(0, unitToMove.Position);
                    //Append the actual destination of the unit, provided you can go to the destination.
                    if(floatLocationCollides(unitToMove, target, Map.cellSize, world))
                        newPath.Add(closestValidCoord(target,Map.cellSize,unitToMove,world));
                    else
                        newPath.Add(target);
                    //Remove all unecessary turns in the path.
                    simplifyPath(unitToMove, world, Map.cellSize, newPath);
                    //Store the path to the unit.
                    unitToMove.path = newPath;
                    return true;
                }

                open.RemoveAt(0);
                explored.Add(currNode);
                foreach (Node neighbor in findNeighbors(unitToMove, currNode, world, goal))
                {
                    if (findNode(neighbor, explored) > -1)
                        continue;
                    tentativeG = currNode.g + 1;

                    if(findNode(neighbor, open) == -1){
                        neighbor.h = hCost(neighbor, startNode.coordinates, goal);
                        chosenIndex = orderedInsert(neighbor, open);
                        costImproved = true;
                    }
                    else if (tentativeG < neighbor.g)
                    {
                        chosenIndex = findNode(neighbor, open);
                        costImproved = true;
                    }
                    else
                    {
                        costImproved = false;
                    }

                    if (costImproved)
                    {
                        open.RemoveAt(chosenIndex);
                        neighbor.previous = currNode;
                        neighbor.g = tentativeG;
                        neighbor.f = neighbor.g + neighbor.h;
                        orderedInsert(neighbor, open);
                    }
                }
            }
            return false;
        }

        private static bool between(int item, int bound1, int bound2)
        {
            //Swap values so bound1 is the lowest.
            if (bound2 < bound1)
            {
                int temp = bound2;
                bound2 = bound1;
                bound1 = temp;
            }
            //Return whether or not item is between the two numbers.
            return item >= bound1 && item <= bound2;
        }

        private static int findNode(Node node, List<Node> items)
        {
            for (int i = 0; i < items.Count; i++ )
            {
                if (items[i].x == node.x && items[i].y == node.y)
                    return i;
            }
            return -1;
        }

        /*
         * hasCollisions: Iteratively finds what side of each square our line crosses through, and checks that square for collisions.
         * 
         *  corner: corner we are looking at for this unit.
         * 
         * possible bug: since these numbers are derived from integers, I may have a case where I run directly through a corner.
         */
        private static bool hasCollisions(Unit myUnit, bool[,] world, int cellSize, Vector2 start, Vector2 goal, Point corner)
        {
            start += new Vector2(corner.X * cellSize, corner.Y * cellSize);
            goal += new Vector2(corner.X * cellSize, corner.Y * cellSize);
            //Convert our line to slope intercept form.
            float m = (goal.Y - start.Y) / (goal.X - start.X);
            float b = goal.Y - m * goal.X;
            Point coords = getCoords(start, cellSize);
            Point goalCoords = getCoords(goal, cellSize);
            Rectangle worldSquare = Rectangle.Empty;
            //If the corner we are doing lands precisely on its  axis.
            if (corner.X != 0 && start.X % cellSize == 0)
                coords.X -= 1;
            if (corner.X != 0 && goal.X % cellSize == 0)
                goalCoords.X -= 1;
            if (corner.Y != 0 && start.Y % cellSize == 0)
                coords.Y -= 1;
            if (corner.Y != 0 && goal.Y % cellSize == 0)
                goalCoords.Y -= 1;
            //Check that we're starting in a valid location, because I use this function for previously unchecked corners of shapes.
            float dx = goal.X - start.X;
            float dy = goal.Y - start.Y;
            if (locationCollides(myUnit, coords, world))
                return true;
            while (coords != goalCoords)
            {
                Vector2 temp = restoreCoords(coords, cellSize);
                dx = goal.X - start.X;
                dy = goal.Y - start.Y;
                worldSquare = new Rectangle((int)temp.X, (int)temp.Y, cellSize, cellSize);
                //Change the currently selected coords. First go through special cases to make sure the line is not vertical or horizontal.
                if (dx == 0 && dy < 0)
                    coords.Y -= 1;
                else if (dx == 0 && dy > 0)
                    coords.Y += 1;
                else if (dy == 0 && dx < 0)
                    coords.X -= 1;
                else if (dy == 0 && dx > 0)
                    coords.X += 1;
                else if (dx == 0 && dy == 0)
                    throw new Exception("I didn't move.");
                else if (crossedTop(worldSquare, m, b, dx))
                    coords.Y -= 1;
                else if (crossedBottom(worldSquare, m, b, dx))
                    coords.Y += 1;
                else if (crossedLeft(worldSquare, m, b, dx))
                    coords.X -= 1;
                else if (crossedRight(worldSquare, m, b, dx))
                    coords.X += 1;
                else if (dx < 0 && dy < 0)//Upper left corner.
                {
                    coords.X -= 1;
                    coords.Y -= 1;
                }
                else if (dx > 0 && dy < 0)//Upper right corner.
                {
                    coords.X += 1;
                    coords.Y -= 1;
                }
                else if (dx < 0 && dy > 0)//Lower left corner.
                {
                    coords.X -= 1;
                    coords.Y += 1;
                }
                else if (dx > 0 && dy > 0)//Lower right corner.
                {
                    coords.X += 1;
                    coords.Y += 1;
                }
                else//This case shouldn't happen, because the line needs to pass through something.
                    return true;
                //Make sure we haven't passed our goal. It would be silly to keep going into infinity.
                if (passedIt(coords, goalCoords, dx, dy))
                    break;
                //Check if the new location will create a collision.
                if (locationCollides(myUnit, coords, world))
                    return true;
            }
            return false;
        }

        /*
         * description: checks if there's any chance of a box passing through a location if it
         *      continues in its current direction.
         */
        private static bool passedIt(Point coords, Point goalCoords, float dirX, float dirY)
        {
            int dx = goalCoords.X-coords.X;
            int dy = goalCoords.Y-coords.Y;
            if (dirX < 0 && dx > 0)
                return true;
            else if (dirX > 0 && dx < 0)
                return true;
            else if (dirY < 0 && dy > 0)
                return true;
            else if (dirY > 0 && dy < 0)
                return true;
            return false;
        }

        private static bool crossedTop(Rectangle worldPos, float m, float b, float dir)
        {
            //If dir is negative, then the unit is moving left.
            if (dir < 0)
            {
                if (m * worldPos.Left + b < worldPos.Top)
                    return true;
            }
            else
            {
                if (m * worldPos.Right + b < worldPos.Top)
                    return true;
            }
            return false;
        }

        private static bool crossedBottom(Rectangle worldPos, float m, float b, float dir)
        {
            //If dir is negative, then the unit is moving left.
            if (dir < 0)
            {
                if (m * worldPos.Left + b > worldPos.Bottom)
                    return true;
            }
            else
            {
                if (m * worldPos.Right + b > worldPos.Bottom)
                    return true;
            }
            return false;
        }

        private static bool crossedLeft(Rectangle worldPos, float m, float b, float dir)
        {
            //If dir is negative, then the unit is moving left.
            if (dir < 0)
            {
                if (m * worldPos.Left + b > worldPos.Top && m * worldPos.Left + b < worldPos.Bottom)
                    return true;
            }
            else
            {
                //It can't cross the left side by moving right.
            }
            return false;
        }

        private static bool crossedRight(Rectangle worldPos, float m, float b, float dir)
        {
            //If dir is negative, then the unit is moving left.
            if (dir < 0)
            {
                //It can't cross the right side by moving left.
            }
            else
            {
                if (m * worldPos.Right + b > worldPos.Top && m * worldPos.Right + b < worldPos.Bottom)
                    return true;
            }
            return false;
        }

        /*
         * description: typically used for my pathfinding algorithm. Just returns whether a unit will
         *      collide anything if placed at the location specified in the world.
         * 
         * unit: the unit to check collision for.
         * 
         * location: the location (in navmesh coordinates) to check collision at.
         * 
         * world: an array of booleans indicating whether various areas of the map are navigable.
         *      True means you can navigate through that area without hitting anything.
         */
        private static bool locationCollides(Unit unit, Point location, bool[,] world)
        {
            //Find the width and height of the unit in navmesh coords.
            for (int x = location.X; x < location.X + unit.size; x++)
            {
                for (int y = location.Y; y < location.Y + unit.size; y++)
                {
                    if (!world[x, y])
                        return true;
                }
            }
            return false;
        }

        /*
         * description: a slightly more expensive version of locationCollides.
         *      This one does not assume the the unit is perfectly on the grid.
         *      
         * location: the location (in world coordinates) to check collision at.
         */
        public static bool floatLocationCollides(Unit unit, Vector2 location, int cellSize, bool[,] world)
        {
            //Find the width and height of the unit in navmesh coords.
            int xOffset = (location.X % cellSize == 0) ? 0:1;
            int yOffset = (location.Y % cellSize == 0) ? 0:1;
            Point upperLeft = getCoords(location, cellSize);
            for (int x = upperLeft.X; x < upperLeft.X + unit.size + xOffset; x += 1)
            {
                for (int y = upperLeft.Y; y < upperLeft.Y + unit.size + yOffset; y += 1)
                {
                    if (!world[x, y])
                        return true;
                }
            }
            return false;
        }

        public static bool floatPointCollides(Vector2 point)
        {
            Point coords = getCoords(point, Map.cellSize);
            if (!world[coords.X, coords.Y])
                return true;
            return false;
        }

        /*
         * description: destined primarily for eternal use, this function returns the closest
         *      coordinate to the given one that will not collide with the navmesh.
         * 
         * invariant: negative positions will never be processed by any part of Collision.cs, except outsideMap().
         *      The point of outsideMap is to find points outsid the map (often negative) and remove them from
         *      the waypoints before I try to process them in here.
         * 
         * not actually a fault: this check concentric squares, if any cell in a square falls between the
         *      current circular radius and the last, check to see if you that's a valid location.
         *      You may think this can result in not checking all cells within a square because the center
         *      of an edge may be 1+ units closer to the center of a square than its corner. Luckily, the
         *      circles I check have radii of the chorter distance, so it's just extra work.
         */
        public static Vector2 closestValidCoord(Vector2 location, int cellSize, Unit unit, bool[,] world)
        {
            //I don't care to check anything if the unit doesn't collide in the given position.
            if (!floatLocationCollides(unit, location, cellSize, world))
                return location;
            //Otherwise, start from the navmesh coordinates the unit seems to want, and migrate out from there.
            Point coords = getCoords(location, cellSize);
            int radius = 1;
            Vector2 closestLoc = new Vector2(-1, -1);
            while (true)
            {
                for (int dx = -radius; dx <= radius; dx++)
                {
                    for (int dy = -radius; dy <= radius; dy++)
                    {
                        Point checkCoord = new Point(coords.X + dx, coords.Y + dy);
                        if (checkCoord.X < 0 || checkCoord.Y < 0 || checkCoord.X >= world.GetLength(0) || checkCoord.Y >= world.GetLength(1))
                            continue;
                        if (withinRadius(coords, radius, checkCoord) && !withinRadius(coords, radius - 1, checkCoord))
                        {
                            Vector2 newLoc = restoreCoords(checkCoord, cellSize);
                            //If this location works, and it's closer to our goal than the last one found, store it.
                            if (!locationCollides(unit, checkCoord, world) && closestLoc == new Vector2(-1, -1))
                                closestLoc = newLoc;
                            else if (!locationCollides(unit, checkCoord, world) && closerToTarget(newLoc, closestLoc, location))
                                closestLoc = newLoc;
                        }
                    }
                }
                if (closestLoc != new Vector2(-1, -1))
                {
                    //Try placing it at the new x-y coordinates with one of the two coordinates replaced by the target coordinate.
                    if (!floatLocationCollides(unit, new Vector2(closestLoc.X, location.Y), cellSize, world))
                        return new Vector2(closestLoc.X, location.Y);
                    else if (!floatLocationCollides(unit, new Vector2(location.X, closestLoc.Y), cellSize, world))
                        return new Vector2(location.X, closestLoc.Y);
                    //If the unit cannot be offset, return the coords precisely on the grid as calculated.
                    return closestLoc;
                }
                radius += 1;
                if (radius > MathHelper.Max(world.GetLength(0), world.GetLength(1)))
                    break;
            }
            //If a valid pair of coordinates cannot be found (but it should be otherwise we probably couldn't get here) then break everything in a rage.
            throw new Exception("There are no spaces big enough for this unit on the entire map! What have you been feeding it?!");
        }

        private static bool closerToTarget(Vector2 p1, Vector2 p2, Vector2 target)
        {
            float dx1 = p1.X - target.X;
            float dx2 = p2.X - target.X;
            float dy1 = p1.Y - target.Y;
            float dy2 = p2.Y - target.Y;
            float dist1 = (float)Math.Sqrt(dx1 * dx1 + dy1 * dy1);
            float dist2 = (float)Math.Sqrt(dx2 * dx2 + dy2 * dy2);
            if (dist1 < dist2)
                return true;
            else
                return false;
        }

        private static bool withinRadius(Point center, int radius, Point coords)
        {
            int dx = coords.X-center.X;
            int dy = coords.Y - center.Y;
            if (dx * dx + dy * dy < radius * radius)
                return true;
            return false;
        }

        /*
         * getCoords: returns coordinates in terms of our nav mesh, which can then be used as indices into the navmesh.
         * 
         * worldCoords: the coordinates in pixels, or fractions of pixels, or whatever we use for our game in most cases.
         * 
         * navMeshDims: the integer dimensions of our navmesh. How many spaces tall and wide it is.
         */
        private static Point getCoords(Vector2 worldCoords, int cellSize)
        {
            return new Point((int)(worldCoords.X / cellSize), (int)(worldCoords.Y / cellSize));
        }

        /*
         * restoreCoords: returns coordinates in terms of the world.
         */
        private static Vector2 restoreCoords(Point navCoords, int cellSize)
        {
            return new Vector2(navCoords.X * cellSize, navCoords.Y * cellSize);
        }

        /*
         * makePath: builds backwards, and returns, the cheapest path taken to get from the start node to a given node.
         * 
         * lastStep: the "end" node you want to arrive at.
         * 
         * cellSize: the pixel size of a cell in our nav mesh.
         */
        private static List<Vector2> makePath(Node lastStep, int cellSize)
        {
            Node currentNode = lastStep;
            List<Vector2> path = new List<Vector2>();
            do
            {
                path.Insert(0, new Vector2(currentNode.x * cellSize, currentNode.y * cellSize));
                currentNode = currentNode.previous;
            } while (currentNode != null);
            return path;
        }

        /*
         * simplifyPath: returns a path with all unnecessary turns removed.
         * 
         * iPath: the initial path, hopefully made up of adjacent nodes.
         * 
         * side effect: changes iPath directly. Doesn't affect any nodes, just the list.
         */
        private static void simplifyPath(Unit myUnit, bool[,] world, int cellSize, List<Vector2> iPath)
        {
            //If the path is shorter than 3 units, then there's really nothing to do.
            if (iPath.Count < 3)
                return;
            //Otherwise continue as normal.
            for (int i = 1; i < iPath.Count - 1; i++)
            {
                //If you can make do without the current node (remember to check all four corners!)... 
                if (!anyCornerPathsHaveCollisions(myUnit, world, cellSize, iPath[i - 1], iPath[i + 1]))
                {
                    //Then who needs it? Remove the node.
                    iPath.RemoveAt(i);
                    //Also decrement i to offset the next iteration's increment.
                    i--;
                }
            }
        }

        private static bool anyCornerPathsHaveCollisions(Unit myUnit, bool[,] world, int cellSize, Vector2 p1, Vector2 p2)
        {
            //If none of the corners run into collisions, then there are no collisions at all.
            if (!hasCollisions(myUnit, world, cellSize, p1, p2, Point.Zero) && //Upper left.
                !hasCollisions(myUnit, world, cellSize, p1, p2, new Point(1, 1)) && //Lower right.
                !hasCollisions(myUnit, world, cellSize, p1, p2, new Point(0, 1)) && //Lower left.
                !hasCollisions(myUnit, world, cellSize, p1, p2, new Point(1, 0))) //Upper right.
                return false;
            return true;
        }

        /*
         * description: hCost is part of a typical A* search algorithm. h is supposed to
         *      be a heurisitc algorithm to determine the value of a node for approaching
         *      a destination. Put more simply, h is the predicted cost to reach a
         *      destination node. The difference is that in my case, h also tries to put
         *      a path on a direct line between two paths.
         */
        private static float hCost(Node node, Point start, Point goal)
        {
            //I'm not sure if object comparison works the same in xna and Java, but I doubt it.
            if (start == goal || node.coordinates == goal)
                return 0;
            //Find out the actual movement cost to reach your destination.
            int costToReach = Math.Abs(node.x - goal.X) + Math.Abs(node.y - goal.Y);
            //If the x values are the same, we need a different approach.
            if (goal.X == start.X)
            {
                //The line is vertical, so if my node is between the y values, then its distance from the line is just flat along the x axis.
                if (between(node.y, start.Y, goal.Y))
                    return costToReach + Math.Abs(node.x - goal.X);
                //Otherwise, we need to do trig.
                //First find out which point is closer (on y axis) to our node.
                Point lesser = (MathHelper.Distance(start.Y, node.y) > MathHelper.Distance(goal.Y, node.y) ? goal : start);
                int dy = Math.Abs(lesser.Y - node.y);
                int dx = Math.Abs(lesser.X - node.x);
                return costToReach + (float)Math.Sqrt(dx * dx + dy * dy);
            }
            double m = (goal.Y - start.Y) / (goal.X - start.X);
            double b = goal.Y - m * goal.X;
            float distFromFavoredPath = (float)(Math.Abs(node.y - m * node.x - b) / Math.Sqrt(Math.Pow(m, 2) + 1));
            return costToReach + distFromFavoredPath;
        }

        //Insert a Node into a list of Nodes, ordered by f value
        private static int orderedInsert(Node item, List<Node> items)
        {
            //List is already ordered if it's empty.
            if (items.Count == 0)
            {
                items.Add(item);
                return 0;
            }
            //No luck. We need to check ordering.
            int min = 0;
            int max = items.Count;
            int check = 0;
            while (min != max)
            {
                check = (max + min) / 2;
                if (item.f > items[check].f)
                {
                    min = check + (max-min == 1 ? 1 : 0);
                }
                else if (item.f < items[check].f)
                {
                    max = check;
                }
                else
                {
                    break;
                }
            }
            //These next two lines rely on the fact that max and min are equal at this point.
            items.Insert(min, item);
            return min;
        }

        private static List<Node> findNeighbors(Unit unit, Node fromNode, bool[,] world, Point goal)
        {
            List<Node> rtnVal = new List<Node>();
            foreach (Point p in new Point[] { new Point(0,1), new Point(0,-1), new Point(1,0), new Point(-1,0)})
            {
                //Make sure the generated locations are on the board.
                if (fromNode.x + p.X < world.GetLength(0) && fromNode.x + p.X >= 0)
                {
                    if (fromNode.y + p.Y < world.GetLength(1) && fromNode.y + p.Y >= 0)
                    {
                        //Make sure we are actually moving something
                        if(p.X == 0 && p.Y == 0)
                        {
                            continue;
                        }
                        //Create a new point from the old location and the change in position.
                        Point newPos = new Point(fromNode.x + p.X, fromNode.y + p.Y);
                        //Make sure we can move units into this area.
                        if (!locationCollides(unit, newPos, world))
                        {
                            //At this point we can create a node
                            rtnVal.Add(new Node(newPos));
                        }
                    }
                }
            }
            return rtnVal;
        }

        public static bool outsideMap(Vector2 point)
        {
            if (point.X < 0 || point.X > world.GetLength(0) * Map.cellSize)
                return true;
            if (point.Y < 0 || point.Y > world.GetLength(1) * Map.cellSize)
                return true;
            return false;
        }

        /*
         * description: sifts the given unit apart from other units.
         */
        public static void siftAgainstUnits(Unit unit)
        {
            foreach (Unit u in allUnits)
            {
                //Can't sift a unit against itself, nor can you against a non-overlapping unit.
                if (u != unit && overlaps(u, unit))
                {
                    moveAsPossible(unit, force(u, unit));
                }
            }
        }

        public static void moveAsPossible(Unit unit, Vector2 movement)
        {
            //Can move left/right and can move up/down respectively.
            bool[] canMove = {true, true};
            if (floatLocationCollides(unit, new Vector2(unit.Position.X + movement.X, unit.Position.Y), Map.cellSize, world))
                canMove[0] = false;
            if (floatLocationCollides(unit, new Vector2(unit.Position.X, unit.Position.Y + movement.Y), Map.cellSize, world))
                canMove[1] = false;
            //If both are possible, but both would result in a collision, only do one.
            if (canMove[0] && canMove[1] && floatLocationCollides(unit, new Vector2(unit.Position.X + movement.X, unit.Position.Y + movement.Y), Map.cellSize, world))
                canMove[rand.Next(1)] = false;
            //Now move!
            if (canMove[0])
                unit.Position += new Vector2(movement.X, 0);
            if(canMove[1])
                unit.Position += new Vector2(0, movement.Y);
        }

        /*
         * description: checks if two units overlap.
         */
        public static bool overlaps(Unit u1, Unit u2)
        {
            //Lets do square collision.
            float dx = u2.Center.X - u1.Center.X;
            float dy = u2.Center.Y - u1.Center.Y;
            float collision_radius = (u1.size + u2.size) * Map.cellSize / 2;
            if (Math.Abs(dx) < collision_radius && Math.Abs(dy) < collision_radius)
                return true;
            return false;
        }

        /*
         * description: returns a force vector on unit 2 from unit 1 when the two are known to overlap.
         */
        public static Vector2 force(Unit u1, Unit u2)
        {
            //Lets do square collision.
            float fx = 0;
            float fy = 0;
            float dx = u2.Center.X - u1.Center.X;
            float dy = u2.Center.Y - u1.Center.Y;
            float collision_radius = (u1.size + u2.size) * Map.cellSize / 2;
            if (Math.Abs(dx) < collision_radius && Math.Abs(dy) < collision_radius && Math.Abs(dx) > SIFT_SPEED && Math.Abs(dy) > SIFT_SPEED)
            {
                fx = Math.Sign(dx) * SIFT_SPEED * (float)rand.NextDouble();
                fy = Math.Sign(dy) * SIFT_SPEED * (float)rand.NextDouble();
                return new Vector2(fx, fy);
            }
            //If we reach this pont then fx and fy are still zero, which means the two units overlap by at least SIFT_SPEED (provided they overlap at all).
            //Return a random force in this case.
            return new Vector2((float)rand.NextDouble() * 2 * SIFT_SPEED - SIFT_SPEED, (float)rand.NextDouble() * 2 * SIFT_SPEED - SIFT_SPEED);
        }

        private class Node
        {
            public float g, h, f;
            public Point coordinates;
            public Node previous;

            public int x
            {
                get { return coordinates.X; }
            }

            public int y
            {
                get { return coordinates.Y; }
            }

            public Node(Point inPos)
            {
                coordinates = new Point(inPos.X, inPos.Y);
                g = 0;
                h = 0;
                f = 0;
                previous = null;
            }
        }
    }
}
